#!/usr/bin/env python
# -*- coding: utf-8 -*-


from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import re
import serial
import numpy as np

import socket

global count_get_uwb  # 只执行一次
global plot_x
global plot_y  # 画图用全局变量坐标
global x
global y
count_get_uwb = 0
plot_x = []
plot_y = []
x = 0
y = 0
#初始化局域网通信
#------------------此处可能会更改端口号-----------
HOST = '192.168.137.59'           #服务器的ip
PORT = 6672                    #服务器端口
connection_string = '/dev/ttyACM0'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect(connection_string, wait_ready=True, baud=921600)
sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)  #以TCP协议模式进行通信
sock.connect((HOST,PORT))         #连接服务器
# 连接的pixhawk飞控的端口


#定义无人机下的坐标系
def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
        0b0000111111000111,  # type_mask
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # m/s
        0, 0, 0,  # x, y, z acceleration
        0, 0)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
# 参数aTargetAltitude即为目标高度，单位为米
def arm_and_takeoff(aTargetAltitude):
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True
    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)
    # 发送起飞指令
    print("Taking off!")
    vehicle.simple_takeoff(1)
    time.sleep(3)
    # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print("Rangefinder distance:", vehicle.rangefinder.distance)
        if vehicle.rangefinder.distance >= 1 * 0.95:
            print("Reached target altitude")
            time.sleep(2)
            break
        else:
            send_body_ned_velocity(0, 0, -0.1, 1)
            print("UP 0.1m")
            time.sleep(1)


def get_uwb():
    global count_get_uwb
    global x
    global y
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)  # change tty/baud rate/timeout here
    while True:
        line = ser.readline()  # read position info
        line = str(line)  # byte->str
        if line:  # if accepted
            a = re.search('KT', line)  # find xyz information
            b = re.search('NULL', line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b:
                    count = line.split(',', 7)  # standard
                    temp1 = count[5].split('(')
                    temp2 = count[7].split(')')
                    position = [0, 0, 0]
                    position[0] = temp1[1]
                    position[1] = count[6]
                    position[2] = temp2[0]
                    temp_x = x  # 上一轮次收到的位置信息
                    temp_y = y
                    x = float(position[1])  # 本轮收到的位置信息
                    y = float(position[0])
                    if count_get_uwb == 0:
                        temp_x = x
                        temp_y = y
                        count_get_uwb = 1
                    s1 = x - temp_x  # 计算两次差值
                    s2 = y - temp_y
                    if (abs(s1) > 1):  # 如果差值大于0.3进行修正
                        x = temp_x + np.sign(s1) * 1
                        print("Value [X] is corrected")
                    if (abs(s2) > 1):
                        y = temp_y + np.sign(s2) * 1
                        print("Value [Y] is corrected")
                    print("Camera position", x, y)
#                     plot_pos(x,y)
                    break
        else:
            break
    return [x, y]


# 以下是坐标飞行模式
def goto_pos(goal):
    print("Start Mission. Goal is:", goal)
    mis = 0.3
    while True:
        ori = get_uwb()  # 当前坐标点
        print("ori", ori)
        actx = goal[0] - ori[0]  # 距离目标的绝对距离
        acty = goal[1] - ori[1]  # 距离目标的绝对距离
        if (actx > mis):  # 判断前进方向
            flag = 1  # 正飞
        elif (abs(actx) < mis):
            flag = 0  #
        elif (actx < -mis):
            flag = -1  # 反飞
        if (acty > mis):  # 判断前进方向
            flag2 = 0.5  # 正飞
            if (acty > mis*2):
                flag2 = 1
        elif (abs(acty) < mis):
            flag2 = 0  #
        elif (acty < -mis):
            flag2 = -0.5  # 反飞
            if (acty < -mis*2):
                flag2 = -1
        send_body_ned_velocity(flag * 0.2, flag2 * 0.2, 0, 1)
        if (flag == 0 and flag2 == 0):  # 距离目标点误差为0.1m以内就可以停止
            break
        time.sleep(1)
    ("ARIVIED")
    final = get_uwb()
    time.sleep(1)


#以下是多坐标点飞行模式
def path_goto(path_array):
    for i in path_array:
        goto_pos(i)


#监听程序
def monitor():
    print("Waiting for position information from server")
    while True:
        x=0
        y=0
        data=sock.recv(1024)
        data1 = data.decode()
        data1 = data1[-11:]
        count = data1.split(',', 3)  # standard
        print(count)
        x = float(count[0])	#读取坐标信息
        y = float(count[1])   
        mode = int(count[2]) #读取模式信息
        print("Received position: No.1 = x:",x,"y:",y)
#         plot_pos(x,y)
        x_2=x-2.5
        y_2=y-2.5
        print("No.2 goal:",x_2,y_2)
        if (mode != 0): #执行模式不为0时，返回坐标和模式信息
            destation=[x_2,y_2]#收到坐标
            return destation,mode#接受坐标以及模式信息


#主程序
mod = 0 #初始化模式为0
while True:
    dest,mod=monitor() #直到接受到信息为止才会执行判断
    if (mod != 0):
        if (mod == 1):#起飞
            arm_and_takeoff(1)# 调用arm_and_takeoff函数，目标高度1m
        if (mod == 2):#定点飞行模式
            goto_pos(dest)
        if (mod == 3):#退出循环，进入降落，关闭ip连接，关闭
            break


# 发送"降落"指令
print("Land")
# 降落，只需将无人机的飞行模式切换成"Land"
vehicle.mode = VehicleMode("LAND")
sock.close()#关闭连接
# plt.close()#注释此行可以让程序结束运行后仍然保持图像
# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()

